Merge somt-tx,ty into Main - #13

someone please correct formatting. just made sure mt2 poses are not implicitly trusted

…dango into sotm_vision

- Extract shootOnTheMove into AutoAlign command (rpm-testing pattern)
- Vision: remove shooter dependency, add getCurrentShot() getter + telemetry
- MegaTag2: add 720 deg/s angular velocity rejection gate
- ShotCalculator: expose resetWarmStart(), called on Y-button gyro reset
- StateMachine: SHOOT_ONLY/INTAKE_DOWN_AND_SHOOT use PRESHOOT (feeder
  waits for flywheel RPM before starting)
- AutoAlign: only requires swerve to avoid conflict with StateMachine
- Constants: add hub tag arrays, distancePID, wiggle position constants

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

- Intake refactored to setWantedState/applyState pattern (from swerve)
- BuildConstants.java removed from tracking, added to .gitignore
- StateMachine: keep PRESHOOT for SHOOT_ONLY and INTAKE_DOWN_AND_SHOOT
- AutoCommands: update AutoAlign constructor to new signature
- Removed stale B*_Hub_HP auto files; retained Set_AutoAlign path

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…sync

- Rotation2d(180.0) was 180 radians (~-172deg); use fromDegrees(180)
- Guard PRESHOOT->SHOOT feeder transition so it won't fire after IDLE
- Sync pose estimator heading when resetGyro() is called
- Ignore docs/ and .claude/ in .gitignore

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Resolved conflict in StateMachine.java: kept swerve's SHOOT_ONLY
(ShooterState.SHOOT, IntakeState.WIGGLING) over sotm_vision's PRESHOOT/IDLE.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…gging

- Add Vision.seedIMU(): forces mode-1 hard seed on both Limelights
  immediately after a gyro reset so MegaTag2 doesn't drift in mode 4
- Y button now sequences resetGyro() -> seedIMU()/resetOffset()/resetWarmStart()
  so seedIMU reads the new 0-degree heading, not the old one
- Fix getTL()/getTY() calls to getLatency_Pipeline() for both cameras

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Raw joystick [-1,1] was passed directly to swerve.drive() which expects
m/s and rad/s. Robot translated at ~22% speed and rotated at ~12% —
effectively nothing. Now matches TeleopSwerve: deadband, cubic scaling,
and maxSpeed/maxAngularVelocity multiplication applied to both axes.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

- Use swerve.getHeading() (gyro) instead of getPose().getRotation() for
  PID measurement so it matches the reference frame swerve.drive() uses
  for field-relative conversion — prevents correction jitter from vision jumps
- Clamp rotationSpeed to ±maxAngularVelocity so large heading errors
  don't steal translation bandwidth via wheel speed desaturation
- Implement end() to zero drive output on trigger release

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…ixed motor control

- Hood angle now derived from ProjectileSimulator exit velocity + kinematic solver
  instead of hand-tuned ShotLUT placeholders
- Fixed moveHoodToSetpoint to apply gear ratio (motorRotationsPerHoodRotation)
- Fixed calculateDesiredAngle hub height from hardcoded 8m to actual 1.40m delta
- Added commandRPM() to ShooterSubsystem so SOTM continuously commands the motor
  (setTargetRPM only stored the value without sending to motor controller)
- D-pad now uses stepHood() with MotionMagic position hold instead of velocity mode
- Hood uses its own MotionMagic constants instead of borrowing IntakeConstants
- Added tuning telemetry: Vision/*, SOTM/*, HoodedShooter/StatorCurrent+MotorRotations

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Replace fixedLaunchAngleDeg with minHoodAngleDeg, maxHoodAngleDeg,
maxTofCeilingS, angleStepDeg. Update HoodedShooterConstants to match
new hood range (12-40 deg). Fix pre-existing AutoCommands compilation
error (missing HoodedShooter parameter).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

…dango into sotm_vision

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Removes ProjectileSimulator-based LUT generation in favor of a
hand-tuned ShotTable. SOTM still works — RPM, hood angle, and TOF
are now read from measured data points in ShotTable.java, which
the team can update after range testing on the field.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…tartup

- resetGyro() now sets gyro to 180° on Red alliance instead of always
  zeroing; fixes field-relative drive and pose being mirrored for Red
- resetPositionToFrontReef() no longer calls resetGyro() (which clobbered
  its own alliance-aware heading); sets gyro yaw directly to target pose
- Vision hard-resets the pose estimator on the first confident multi-tag
  reading (2+ tags, avgTagDist < POSE_INIT_MAX_TAG_DIST_M) so the robot
  doesn't spend several seconds converging from field origin (0, 0, 0°);
  calls seedIMU() after reset to re-sync Limelight IMUs immediately
- Extract magic 4.0m threshold into Constants.Vision.POSE_INIT_MAX_TAG_DIST_M

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…dango into sotm_vision

- Vision.periodic() and seedIMU() now use swerve.getHeading() (raw gyro)
  instead of swerve.getPose().getRotation() (estimator heading) when
  calling SetRobotOrientation. Using the estimator heading created a
  feedback loop: wrong vision → wrong heading → worse MegaTag2 → drift.
- Revert startup hard-reset (setPose from first vision reading) which
  amplified bad MegaTag2 poses instead of filtering them.
- Remove unused Constants.Vision fields: ERROR_DEGREES, distancePID,
  targetDistanceMeters, cameraPosePrimary, cameraPoseSecondary.
- Remove unused RED_HUB_TAGS / BLUE_HUB_TAGS — MegaTag2 uses all tags.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

gyro.setYaw() is async (CAN bus) but setPose() immediately reads the
stale gyroYaw, computing a wrong internal offset. When the gyro finally
updates, the estimator heading jumps wildly.

Fix: pass the target yaw directly to poseEstimator.resetPosition()
in resetGyro(), resetHeading(), and resetPositionToFrontReef(),
bypassing the stale getGyroYaw() read.

Also:
- getHeading() returns estimator heading (for driving/autoalign)
- Vision uses getGyroYaw() directly for MT2 seeding (no feedback loop)
- resetHeading() now also resets the gyro to stay in sync
- Removed unused EstimatedRobotPose import and stale Javadoc

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Three locations still read getGyroYaw() immediately after an async
gyro.setYaw() CAN call, producing stale values that corrupt the pose
estimator's internal offset:

- Swerve constructor: use Rotation2d(0) directly instead of stale read
- setPose(): refresh gyroYaw signal before reading (PathPlanner auto start)
- seedIMU(): accept heading parameter instead of reading stale gyro;
  RobotContainer passes the known-correct target yaw

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Gyro resets:
- Stop calling gyro.setYaw() entirely. The pose estimator handles
  heading offsets internally via resetPosition(currentGyro, positions,
  desiredPose). This eliminates the CAN async race condition that caused
  wild heading drift on gyro reset.

MT2 sanity checks (per Limelight/WPILib/CTRE docs):
- Reject poses outside field boundary (0-16.54m X, 0-8.07m Y)
- Reject poses that jump >1m from current estimate
- Existing: reject when spinning >360 deg/s, null, or tagCount==0

Std devs:
- Use community formula: 0.5 * avgTagDist² / tagCount
- Heading std dev remains 9999999 (trust gyro, not vision heading)

seedIMU:
- Use estimator heading (which reflects the desired heading after
  resetGyro) instead of raw gyro (which hasn't changed since we
  no longer call setYaw).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

…td devs

Merges worktree-mt2-fixes into sotm_vision, combining:
- Remove all gyro.setYaw() calls to eliminate CAN race conditions
- Add MT2 pose sanity checks (field bounds, >1m jump rejection)
- Use community std dev formula (0.5 * avgTagDist² / tagCount)
- seedIMU(double) accepts explicit heading parameter

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

…jection

- Add MT1 heading bootstrap for arbitrary starting positions: on first
  boot, use MegaTag1 multi-tag observations (which solve heading from
  tag geometry) to seed gyro offset before switching to MT2
- Add gyroOffset field in Swerve for correct field heading after resets
  without setYaw() CAN race or estimator feedback loop
- Fix jump distance check blocking initial localization (was 1m hard
  threshold; now 1m single-tag, 20m multi-tag)
- Add single-tag ambiguity rejection (>0.7) to filter noisy detections
- Tighten maxScoringDistance from 15m to 5.5m to match LUT range
- Fix SOTM angular velocity feedforward using raw hub position instead
  of velocity-compensated aim point
- Fix heading error telemetry wrapping in AutoAlign
- Move rotationPID ownership from Vision to AutoAlign (private instance)
- Add per-cycle rejection counters for field debugging

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

- Rename headingBootstrapped → poseBootstrapped (it resets full pose)
- Add requestRebootstrap() to re-run MT1 localization after auto ends,
  so the robot snaps back to reality if auto path didn't finish cleanly
- Call requestRebootstrap() from teleopInit() via RobotContainer hook
- Trust multi-tag AprilTag poses more aggressively:
    2+ tags: 0.3 * avgDist / tagCount (linear, e.g. 2 tags @ 2m → 0.3m)
    1  tag:  0.5 * avgDist²           (quadratic, conservative)

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

- Clamp vision distance upper bound to 4.0m to match LUT data range
- Remove duplicate getPose() call in Swerve.periodic()
- Add shooter and hoodedShooter as AutoAlign command requirements

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>